{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import copy\n",
    "import math\n",
    "import itertools\n",
    "import numpy as np \n",
    "import cvxpy as cp\n",
    "import matplotlib.pyplot as plt\n",
    "from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \\\n",
    "                             calc_input, observation, motion_model, Edge, pi_2_pi\n",
    "\n",
    "%matplotlib inline\n",
    "np.set_printoptions(precision=3, suppress=True)\n",
    "np.random.seed(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Introduction\n",
    "\n",
    "In contrast to the probabilistic approaches for solving SLAM, such as EKF, UKF, particle filters, and so on, the graph technique formulates the SLAM as an optimization problem.  It is mostly used to solve the full SLAM problem in an offline fashion, i.e. optimize all the poses of the robot after the path has been traversed. However, some variants are availble that uses graph-based approaches to perform online estimation or to solve for a subset of the poses.\n",
    "\n",
    "GraphSLAM uses the motion information as well as the observations of the environment to create least square problem that can be solved using standard optimization techniques.\n",
    "\n",
    "### Minimal Example\n",
    "The following example illustrates the main idea behind graphSLAM. \n",
    "A simple case of a 1D robot is considered that can only move in 1 direction. The robot is commanded to move forward with a control input $u_t=1$, however, the motion is not perfect and the measured odometry will deviate from the true path. At each timestep the robot can observe its environment, for this simple case as well, there is only a single landmark at coordinates $x=3$. The measured observations are the range between the robot and landmark. These measurements are also subjected to noise. No bearing information is required since this is a 1D problem.\n",
    "\n",
    "To solve this, graphSLAM creates what is called as the system information matrix $\\Omega$ also referred to as $H$ and the information vector $\\xi$ also known as $b$. The entries are created based on the information of the motion and the observation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The determinant of H:  0.0\n",
      "The determinant of H after anchoring constraint:  18.75\n",
      "Running graphSLAM ...\n",
      "Odometry values after optimzation: \n",
      " [[0.   ]\n",
      " [0.947]\n",
      " [1.939]]\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "R = 0.2\n",
    "Q = 0.2\n",
    "N = 3\n",
    "graphics_radius = 0.1\n",
    "\n",
    "odom = np.empty((N,1))\n",
    "obs = np.empty((N,1))\n",
    "x_true = np.empty((N,1))\n",
    "\n",
    "landmark = 3\n",
    "# Simulated readings of odometry and observations\n",
    "x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9\n",
    "x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0\n",
    "x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0\n",
    "\n",
    "hxDR = copy.deepcopy(odom)\n",
    "# Visualization\n",
    "plt.plot(landmark,0, '*k', markersize=30)\n",
    "for i in range(N):\n",
    "    plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')\n",
    "    plt.plot([odom[i], odom[i] + graphics_radius],\n",
    "             [0,0], 'r')\n",
    "    plt.text(odom[i], 0.02, \"X_{}\".format(i), fontsize=12)\n",
    "    plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')\n",
    "    plt.plot(x_true[i],0,'.g', markersize=20)\n",
    "plt.grid()    \n",
    "plt.show()\n",
    "\n",
    "\n",
    "# Defined as a function to facilitate iteration\n",
    "def get_H_b(odom, obs):\n",
    "    \"\"\"\n",
    "    Create the information matrix and information vector. This implementation is \n",
    "    based on the concept of virtual measurement i.e. the observations of the\n",
    "    landmarks are converted into constraints (edges) between the nodes that\n",
    "    have observed this landmark.\n",
    "    \"\"\"\n",
    "    measure_constraints = {}\n",
    "    omegas = {}\n",
    "    zids = list(itertools.combinations(range(N),2))\n",
    "    H = np.zeros((N,N))\n",
    "    b = np.zeros((N,1))\n",
    "    for (t1, t2) in zids:\n",
    "        x1 = odom[t1]\n",
    "        x2 = odom[t2]\n",
    "        z1 = obs[t1]\n",
    "        z2 = obs[t2]\n",
    "        \n",
    "        # Adding virtual measurement constraint\n",
    "        measure_constraints[(t1,t2)] = (x2-x1-z1+z2)\n",
    "        omegas[(t1,t2)] = (1 / (2*Q))\n",
    "        \n",
    "        # populate system's information matrix and vector\n",
    "        H[t1,t1] += omegas[(t1,t2)]\n",
    "        H[t2,t2] += omegas[(t1,t2)]\n",
    "        H[t2,t1] -= omegas[(t1,t2)]\n",
    "        H[t1,t2] -= omegas[(t1,t2)]\n",
    "\n",
    "        b[t1] += measure_constraints[(t1,t2)]\n",
    "        b[t2] -= measure_constraints[(t1,t2)]\n",
    "\n",
    "    return H, b\n",
    "\n",
    "\n",
    "H, b = get_H_b(odom, obs)\n",
    "print(\"The determinant of H: \", np.linalg.det(H))\n",
    "H[0,0] += 1 # np.inf ?\n",
    "print(\"The determinant of H after anchoring constraint: \", np.linalg.det(H))\n",
    "\n",
    "for i in range(5):\n",
    "    H, b = get_H_b(odom, obs)\n",
    "    H[(0,0)] += 1\n",
    "    \n",
    "    # Recover the posterior over the path\n",
    "    dx = np.linalg.inv(H) @ b\n",
    "    odom += dx\n",
    "    # repeat till convergence\n",
    "print(\"Running graphSLAM ...\")    \n",
    "print(\"Odometry values after optimzation: \\n\", odom)\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(x_true, np.zeros(x_true.shape), '.', markersize=20, label='Ground truth')\n",
    "plt.plot(odom, np.zeros(x_true.shape), '.', markersize=20, label='Estimation')\n",
    "plt.plot(hxDR, np.zeros(x_true.shape), '.', markersize=20, label='Odom')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In particular, the tasks are split into 2 parts, graph construction, and graph optimization. \n",
    "### Graph Construction\n",
    "\n",
    "Firstly the nodes are defined $\\boldsymbol{x} = \\boldsymbol{x}_{1:n}$  such that each node is the pose of the robot at time $t_i$\n",
    "Secondly, the edges i.e. the constraints, are constructed according to the following conditions:\n",
    "\n",
    "* robot moves from $\\boldsymbol{x}_i$ to $\\boldsymbol{x}_j$. This edge corresponds to the odometry measurement. Relative motion constraints (Not included in the previous minimal example).\n",
    "* Measurement constraints, this can be done in two ways:\n",
    "    * The information matrix is set in such a way that it includes the landmarks in the map as well. Then the constraints can be entered in a straightforward fashion between a node $\\boldsymbol{x}_i$ and some landmark $m_k$\n",
    "    * Through creating a virtual measurement among all the node that have observed the same landmark. More concretely, robot observes the same landmark from $\\boldsymbol{x}_i$ and $\\boldsymbol{x}_j$. Relative measurement constraint. The \"virtual measurement\" $\\boldsymbol{z}_{ij}$, which is the estimated pose of $\\boldsymbol{x}_j$ as seen from the node $\\boldsymbol{x}_i$. The virtual measurement can then be entered in the information matrix and vector in a similar fashion to the motion constraints.\n",
    "\n",
    "An edge is fully characterized by the values of the error (entry to information vector) and the local information matrix (entry to the system's information vector). The larger the local information matrix (lower $Q$ or $R$) the values that this edge will contribute with.\n",
    "\n",
    "Important Notes:\n",
    "\n",
    "* The addition to the information matrix and vector are added to the earlier values.\n",
    "* In the case of 2D robot, the constraints will be non-linear, therefore, a Jacobian of the error w.r.t the states is needed when updated $H$ and $b$.\n",
    "* The anchoring constraint is needed in order to avoid having a singular information matri.\n",
    "\n",
    "\n",
    "### Graph Optimization\n",
    "\n",
    "The result from this formulation yields an overdetermined system of equations.\n",
    "The goal after constructing the graph is to find: $x^* = \\underset{x}{\\mathrm{argmin}} ~ \\underset{ij} \\Sigma ~ f(e_{ij}) $, where $f$ is some error function that depends on the edges between to related nodes $i$ and $j$. The derivation in the references arrive at the solution for $x^* = H^{-1}b$ \n",
    "\n",
    "\n",
    "### Planar Example:\n",
    "\n",
    "Now we will go through an example with a more realistic case of a 2D robot with 3DoF, namely, $[x, y, \\theta]^T$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "#  Simulation parameter\n",
    "Qsim = np.diag([0.01, np.deg2rad(0.010)])**2 # error added to range and bearing\n",
    "Rsim = np.diag([0.1, np.deg2rad(1.0)])**2 # error added to [v, w]\n",
    "\n",
    "DT = 2.0  # time tick [s]\n",
    "SIM_TIME = 100.0  # simulation time [s]\n",
    "MAX_RANGE = 30.0  # maximum observation range\n",
    "STATE_SIZE = 3  # State size [x,y,yaw]\n",
    "\n",
    "# TODO: Why not use Qsim ? \n",
    "# Covariance parameter of Graph Based SLAM\n",
    "C_SIGMA1 = 0.1\n",
    "C_SIGMA2 = 0.1\n",
    "C_SIGMA3 = np.deg2rad(1.0)\n",
    "\n",
    "MAX_ITR = 20  # Maximum iteration during optimization\n",
    "timesteps = 1\n",
    "\n",
    "# consider only 2 landmarks for simplicity\n",
    "# RFID positions [x, y, yaw]\n",
    "RFID = np.array([[10.0, -2.0, 0.0],\n",
    "#                  [15.0, 10.0, 0.0],\n",
    "#                  [3.0, 15.0, 0.0],\n",
    "#                  [-5.0, 20.0, 0.0],\n",
    "#                  [-5.0, 5.0, 0.0]\n",
    "                 ])\n",
    "\n",
    "# State Vector [x y yaw v]'\n",
    "xTrue = np.zeros((STATE_SIZE, 1))\n",
    "xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning\n",
    "xTrue[2] = np.deg2rad(45)\n",
    "xDR[2] = np.deg2rad(45)\n",
    "# history initial values\n",
    "hxTrue = xTrue\n",
    "hxDR = xTrue\n",
    "_, z, _, _ = observation(xTrue, xDR, np.array([[0,0]]).T, RFID)\n",
    "hz = [z]\n",
    "\n",
    "for i in range(timesteps):\n",
    "    u = calc_input()\n",
    "    xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)\n",
    "    hxDR = np.hstack((hxDR, xDR))\n",
    "    hxTrue = np.hstack((hxTrue, xTrue))\n",
    "    hz.append(z)\n",
    "\n",
    "# visualize\n",
    "graphics_radius = 0.3\n",
    "plt.plot(RFID[:, 0], RFID[:, 1], \"*k\", markersize=20)\n",
    "plt.plot(hxDR[0, :], hxDR[1, :], '.', markersize=50, alpha=0.8, label='Odom')\n",
    "plt.plot(hxTrue[0, :], hxTrue[1, :], '.', markersize=20, alpha=0.6, label='X_true')\n",
    "\n",
    "for i in range(hxDR.shape[1]):\n",
    "    x = hxDR[0, i]\n",
    "    y = hxDR[1, i]\n",
    "    yaw = hxDR[2, i]\n",
    "    plt.plot([x, x + graphics_radius * np.cos(yaw)],\n",
    "             [y, y + graphics_radius * np.sin(yaw)], 'r')\n",
    "    d = hz[i][:, 0]\n",
    "    angle = hz[i][:, 1]\n",
    "    plt.plot([x + d * np.cos(angle + yaw)], [y + d * np.sin(angle + yaw)], '.',\n",
    "             markersize=20, alpha=0.7)\n",
    "    plt.legend()\n",
    "plt.grid()    \n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Copy the data to have a consistent naming with the .py file\n",
    "zlist = copy.deepcopy(hz)\n",
    "x_opt = copy.deepcopy(hxDR)\n",
    "xlist = copy.deepcopy(hxDR)\n",
    "number_of_nodes = x_opt.shape[1]\n",
    "n = number_of_nodes * STATE_SIZE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "After the data has been saved, the graph will be constructed by looking at each pair for nodes. The virtual measurement is only created if two nodes have observed the same landmark at different points in time. The next cells are a walk through for a single iteration of graph construction -> optimization -> estimate update. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Node combinations: \n",
      " [(0, 1)]\n",
      "Node 0 observed landmark with ID 0.0\n",
      "Node 1 observed landmark with ID 0.0\n"
     ]
    }
   ],
   "source": [
    "# get all the possible combination of the different node\n",
    "zids = list(itertools.combinations(range(len(zlist)), 2))\n",
    "print(\"Node combinations: \\n\", zids)\n",
    "\n",
    "for i in range(xlist.shape[1]):\n",
    "    print(\"Node {} observed landmark with ID {}\".format(i, zlist[i][0, 3]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the following code snippet the error based on the virtual measurement between node 0 and 1 will be created.\n",
    "The equations for the error is as follows: \n",
    "$e_{ij}^x = x_j + d_j cos(\\psi_j + \\theta_j) - x_i - d_i cos(\\psi_i + \\theta_i) $\n",
    "\n",
    "$e_{ij}^y = y_j + d_j sin(\\psi_j + \\theta_j) - y_i - d_i sin(\\psi_i + \\theta_i) $\n",
    "\n",
    "$e_{ij}^x = \\psi_j + \\theta_j - \\psi_i - \\theta_i $\n",
    "\n",
    "Where $[x_i, y_i, \\psi_i]$ is the pose for node $i$ and similarly for node $j$, $d$ is the measured distance at nodes $i$ and $j$, and $\\theta$ is the measured bearing to the landmark. The difference is visualized with the figure in the next cell.\n",
    "\n",
    "In case of perfect motion and perfect measurement the error shall be zero since $ x_j + d_j cos(\\psi_j + \\theta_j)$ should equal $x_i + d_i cos(\\psi_i + \\theta_i)$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.0 1.427649841628278 -2.0126109674819155 -3.524048014922737\n",
      "For nodes (0, 1)\n",
      "Added edge with errors: \n",
      " [[-0.02 ]\n",
      " [-0.084]\n",
      " [ 0.   ]]\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Initialize edges list\n",
    "edges = []\n",
    "\n",
    "# Go through all the different combinations\n",
    "for (t1, t2) in zids:\n",
    "    x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]\n",
    "    x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]\n",
    "    \n",
    "    # All nodes have valid observation with ID=0, therefore, no data association condition\n",
    "    iz1 = 0\n",
    "    iz2 = 0\n",
    "    \n",
    "    d1 = zlist[t1][iz1, 0]\n",
    "    angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]\n",
    "    d2 = zlist[t2][iz2, 0]\n",
    "    angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]\n",
    "    \n",
    "    # find angle between observation and horizontal \n",
    "    tangle1 = pi_2_pi(yaw1 + angle1)\n",
    "    tangle2 = pi_2_pi(yaw2 + angle2)\n",
    "    \n",
    "    # project the observations \n",
    "    tmp1 = d1 * math.cos(tangle1)\n",
    "    tmp2 = d2 * math.cos(tangle2)\n",
    "    tmp3 = d1 * math.sin(tangle1)\n",
    "    tmp4 = d2 * math.sin(tangle2)\n",
    "    \n",
    "    edge = Edge()\n",
    "    print(y1,y2, tmp3, tmp4)\n",
    "    # calculate the error of the virtual measurement\n",
    "    # node 1 as seen from node 2 throught the observations 1,2\n",
    "    edge.e[0, 0] = x2 - x1 - tmp1 + tmp2\n",
    "    edge.e[1, 0] = y2 - y1 - tmp3 + tmp4\n",
    "    edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - tangle1 + tangle2)\n",
    "\n",
    "    edge.d1, edge.d2 = d1, d2\n",
    "    edge.yaw1, edge.yaw2 = yaw1, yaw2\n",
    "    edge.angle1, edge.angle2 = angle1, angle2\n",
    "    edge.id1, edge.id2 = t1, t2\n",
    "    \n",
    "    edges.append(edge)\n",
    "    \n",
    "    print(\"For nodes\",(t1,t2))\n",
    "    print(\"Added edge with errors: \\n\", edge.e)\n",
    "    \n",
    "    # Visualize measurement projections\n",
    "    plt.plot(RFID[0, 0], RFID[0, 1], \"*k\", markersize=20)\n",
    "    plt.plot([x1,x2],[y1,y2], '.', markersize=50, alpha=0.8)\n",
    "    plt.plot([x1, x1 + graphics_radius * np.cos(yaw1)],\n",
    "             [y1, y1 + graphics_radius * np.sin(yaw1)], 'r')\n",
    "    plt.plot([x2, x2 + graphics_radius * np.cos(yaw2)],\n",
    "             [y2, y2 + graphics_radius * np.sin(yaw2)], 'r')\n",
    "    \n",
    "    plt.plot([x1,x1+tmp1], [y1,y1], label=\"obs 1 x\")\n",
    "    plt.plot([x2,x2+tmp2], [y2,y2], label=\"obs 2 x\")\n",
    "    plt.plot([x1,x1], [y1,y1+tmp3], label=\"obs 1 y\")\n",
    "    plt.plot([x2,x2], [y2,y2+tmp4], label=\"obs 2 y\")\n",
    "    plt.plot(x1+tmp1, y1+tmp3, 'o')\n",
    "    plt.plot(x2+tmp2, y2+tmp4, 'o')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the constraints equations derived before are non-linear, linearization is needed before we can insert them into the information matrix and information vector. Two jacobians \n",
    "\n",
    "$A = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_i}$ as $\\boldsymbol{x}_i$ holds the three variabls x, y, and theta.\n",
    "Similarly, \n",
    "$B = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_j}$ "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The determinant of H:  0.0\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The determinant of H after origin constraint:  716.1972439134918\n"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Initialize the system information matrix and information vector\n",
    "H = np.zeros((n, n))\n",
    "b = np.zeros((n, 1))\n",
    "x_opt = copy.deepcopy(hxDR)\n",
    "\n",
    "for edge in edges:\n",
    "    id1 = edge.id1 * STATE_SIZE\n",
    "    id2 = edge.id2 * STATE_SIZE\n",
    "    \n",
    "    t1 = edge.yaw1 + edge.angle1\n",
    "    A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)],\n",
    "                  [0, -1.0, -edge.d1 * math.cos(t1)],\n",
    "                  [0, 0, -1.0]])\n",
    "\n",
    "    t2 = edge.yaw2 + edge.angle2\n",
    "    B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)],\n",
    "                  [0, 1.0, edge.d2 * math.cos(t2)],\n",
    "                  [0, 0, 1.0]])\n",
    "    \n",
    "    # TODO: use Qsim instead of sigma\n",
    "    sigma = np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3])\n",
    "    Rt1 = calc_rotational_matrix(tangle1)\n",
    "    Rt2 = calc_rotational_matrix(tangle2)\n",
    "    edge.omega = np.linalg.inv(Rt1 @ sigma @ Rt1.T + Rt2 @ sigma @ Rt2.T)\n",
    "\n",
    "    # Fill in entries in H and b\n",
    "    H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A\n",
    "    H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B\n",
    "    H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A\n",
    "    H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B\n",
    "\n",
    "    b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)\n",
    "    b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)\n",
    "   \n",
    "\n",
    "print(\"The determinant of H: \", np.linalg.det(H))\n",
    "plt.figure()\n",
    "plt.subplot(1,2,1)\n",
    "plt.imshow(H, extent=[0, n, 0, n])\n",
    "plt.subplot(1,2,2)\n",
    "plt.imshow(b, extent=[0, 1, 0, n])\n",
    "plt.show()    \n",
    "\n",
    "# Fix the origin, multiply by large number gives same results but better visualization\n",
    "H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)\n",
    "print(\"The determinant of H after origin constraint: \", np.linalg.det(H))    \n",
    "plt.figure()\n",
    "plt.subplot(1,2,1)\n",
    "plt.imshow(H, extent=[0, n, 0, n])\n",
    "plt.subplot(1,2,2)\n",
    "plt.imshow(b, extent=[0, 1, 0, n])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "dx: \n",
      " [[ 0.   ]\n",
      " [-0.   ]\n",
      " [ 0.   ]\n",
      " [ 0.02 ]\n",
      " [ 0.084]\n",
      " [-0.   ]]\n",
      "ground truth: \n",
      "  [[0.    1.414]\n",
      " [0.    1.414]\n",
      " [0.785 0.985]]\n",
      "Odom: \n",
      " [[0.    1.428]\n",
      " [0.    1.428]\n",
      " [0.785 0.976]]\n",
      "SLAM: \n",
      " [[ 0.     1.448]\n",
      " [-0.     1.512]\n",
      " [ 0.785  0.976]]\n",
      "\n",
      "graphSLAM localization error:  0.0107290727510571\n",
      "Odom localization error:  0.0004460978857535104\n"
     ]
    }
   ],
   "source": [
    "# Find the solution (first iteration)\n",
    "dx = - np.linalg.inv(H) @ b\n",
    "for i in range(number_of_nodes):\n",
    "    x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]\n",
    "print(\"dx: \\n\",dx)\n",
    "print(\"ground truth: \\n \",hxTrue)\n",
    "print(\"Odom: \\n\", hxDR)\n",
    "print(\"SLAM: \\n\", x_opt)\n",
    "\n",
    "# performance will improve with more iterations, nodes and landmarks.\n",
    "print(\"\\ngraphSLAM localization error: \", np.sum((x_opt - hxTrue) ** 2))\n",
    "print(\"Odom localization error: \", np.sum((hxDR - hxTrue) ** 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The references:\n",
    "\n",
    "\n",
    "* http://robots.stanford.edu/papers/thrun.graphslam.pdf\n",
    "\n",
    "* http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/16-graph-slam.pdf\n",
    "\n",
    "* http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf\n",
    "\n",
    "N.B.\n",
    "An additional step is required that uses the estimated path to update the belief regarding the map."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
